#include <motor/servo_zapi.h>

namespace ros_canopen_driver {

ServoZapi::ServoZapi(std::shared_ptr<rclcpp::Node> node, const std::string name)
{
    node_ = node;
    joint_name = name;
    servo_manufacturer = ZAPI;
    memset(&m_servo_motor, 0, sizeof(SERVOMOTOR_PORT));
    motorState = Off;
    pc_.timestamp = node_->now().seconds();
}


ServoZapi::~ServoZapi()
{
    encoder = nullptr;
}


/*Description: load motor configuration parameters from yaml file*/
bool ServoZapi::Motor_InitParam()
{
   
    if (!node_->has_parameter(joint_name + "/nodeId")) {
        RCLCPP_ERROR_STREAM(node_->get_logger(), joint_name << " nodeId is invalid");
        return false;
    }
    int nodeId;
    node_->get_parameter(joint_name + "/nodeId", nodeId);
    m_servo_motor.cfg.nodeId = nodeId;
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " nodeId is " << nodeId);
    node_->get_parameter_or(joint_name + "/busname", busname, std::string("can0"));
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " busname: " << busname);
    int ctl_mode;
    node_->get_parameter_or(joint_name + "/ctl_mode", ctl_mode, 0);
    m_servo_motor.cfg.ctl_mode = ctl_mode;
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " ctl_mode is " << ctl_mode);
    node_->get_parameter_or(joint_name + "/rotateReverse", m_servo_motor.cfg.rotateReverse, false);
    node_->get_parameter_or(joint_name + "/reduction_ratio", m_servo_motor.cfg.reduction_ratio, 1.0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " reduction_ratio is " << m_servo_motor.cfg.reduction_ratio);
    node_->get_parameter_or(joint_name + "/encoder_resolution", m_servo_motor.cfg.encoder_resolution, 100.0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " encoder_resolution is " << m_servo_motor.cfg.encoder_resolution);
    node_->get_parameter_or(joint_name + "/use_extra_encoder", use_extra_encoder, false);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " use external encoder: " << use_extra_encoder);
    node_->get_parameter_or(joint_name + "/pos_scale", pos_scale, 1.0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " pos_scale: " << pos_scale);
    node_->get_parameter_or(joint_name + "/pos_offset", pos_offset, 0.0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name << " pos_offset: " << pos_offset);
    
    return true;
}


int ServoZapi::Motor_Init()
{
    if (use_extra_encoder) {
        encoder = encoder::EncoderFactory::generateEncoder(node_, joint_name, encoder::CURTIS_ENCODER);
        encoder->init();
    }

    uint8_t nodeId = m_servo_motor.cfg.nodeId;
    if((0 == nodeId) || (nodeId > 127))
    {
        RCLCPP_ERROR(node_->get_logger(), "[SERVO_MOTOR]motor nodeId is unvaild");
        return -1;       
    }

    set_vel_pub = node_->create_publisher<std_msgs::msg::UInt16>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_target_speed", 10);
        
    set_control_word_pub = node_->create_publisher<std_msgs::msg::UInt8>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_control_word", 10);
        
    set_io_output_pub = node_->create_publisher<std_msgs::msg::UInt8>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_io_output", 10);

    current_vel_sub = node_->create_subscription<std_msgs::msg::Int16>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/get_actual_speed", 
        10, 
        std::bind(&ServoZapi::current_vel_sub_callback, this, std::placeholders::_1));

    errcode_sub = node_->create_subscription<std_msgs::msg::UInt16>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/get_warning_info", 
        10, 
        std::bind(&ServoZapi::errcode_sub_callback, this, std::placeholders::_1));

    di_sub = node_->create_subscription<std_msgs::msg::UInt16>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/get_io_input", 
        10, 
        std::bind(&ServoZapi::di_sub_callback, this, std::placeholders::_1));

    di_interface_pub = node_->create_publisher<std_msgs::msg::UInt16>(
        "/zapi_digital_input", 10);

    custom_operation_sub = node_->create_subscription<agv_msgs::msg::PeriphCustomOperation>(
        "/zapi_periph_operation", 
        10, 
        std::bind(&ServoZapi::custom_operation_sub_callback, this, std::placeholders::_1));


    RCLCPP_INFO(node_->get_logger(), "node[%d] enable.", nodeId);
    // motor enable
    bool enableFlage = Motor_Enable(true);
    if(!enableFlage)
    {
        return -1;
    }

    // ctlmode
    if(m_servo_motor.cfg.ctl_mode == Profiled_Velocity)
    {
        RCLCPP_INFO(node_->get_logger(), "node[%d] set velocity mode.", nodeId);
        if(!Motor_InitVmode())
            return -1;
    }
    else
    {
        RCLCPP_ERROR(node_->get_logger(), "[SERVO_MOTOR]unknown drive mode");
        return -1;
    }
    motorState = Ready;        
    return 0;

}

bool ServoZapi::Motor_Enable(bool onFlag)
{
    std_msgs::msg::UInt8 msg;
    
    if (onFlag) {
        msg.data = (uint8_t)0x07;
        set_control_word_pub->publish(msg);
    }
    else {
        msg.data = (uint8_t)0x00;
        set_control_word_pub->publish(msg);
    }
    return 1;
}

bool ServoZapi::Motor_InitVmode()
{
    Motor_SetTargetVelEnc(0);
    return true;
}

bool ServoZapi::Motor_InitPmode()
{
    return true;
}

/*Description: set the speed for Profiled Velocity mode, vel is expressed in rad/s*/
void ServoZapi::Motor_SetTargetVelRad(double vel)
{
    if(Ready != Motor_GetState())
    {
        RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
            1000, "[ServoZapi] motor[%s] state[%d] not Ready !!!", joint_name.c_str(), Motor_GetState());
        return;
    }

    if (std::isnan(vel) || abs(vel) > 1e9) {
        RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
            1000, "[ServoZapi] bad vel value: %.4f", vel);
        Motor_SetTargetVelEnc(0);
        return;
    }

    // rad/s to rpm
    // vel = vel * 30 / M_PI;

    vel = vel * 60 * m_servo_motor.cfg.reduction_ratio * m_servo_motor.cfg.encoder_resolution / (30.0 * 2.0 * M_PI);

    Motor_SetTargetVelEnc(vel);
}

void ServoZapi::Motor_SetPmodeVel(uint32_t vel)
{
    return;   
}


/*Description: set the speed for Profiled Velocity mode, vel is expressed in rpm*/
void ServoZapi::Motor_SetTargetVelEnc(int vel)
{
    std_msgs::msg::UInt8 ctlWord_msg;
    std_msgs::msg::UInt16 vel_msg;
    
    if (vel == 0) {
        ctlWord_msg.data = (uint8_t)0x07;
    }
    else if (vel > 0) {
        ctlWord_msg.data = (uint8_t)0x47;
    }
    else {
        ctlWord_msg.data = (uint8_t)0x87;
    }
    
    set_control_word_pub->publish(ctlWord_msg);
    
    vel_msg.data = abs(vel); //rpm to hz
    RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
        1000, "[ServoZapi] set target vel enc: %d", vel);
    set_vel_pub->publish(vel_msg);
}

/*Description: get the motor speed ignore control mode, vel is expressed in rad per second*/
double ServoZapi::Motor_GetCurrentVel()
{
    int vel_enc = servo_VvelGet();
    
    double vel = vel_enc * 2.0 * M_PI * 30.0 / (m_servo_motor.cfg.reduction_ratio * m_servo_motor.cfg.encoder_resolution * 60);

    return vel;
}

void ServoZapi::Motor_SetTargetPosRad(double pos)
{
    return;
}


int8_t ServoZapi::Motor_GetMode()
{
    return 3;
}

/*Description: get the motor speed ignore control mode*/
int ServoZapi::servo_velGet()
{
    if(m_servo_motor.cfg.ctl_mode == Profiled_Position)
    {
        return servo_PvelGet();
    }
    else if(m_servo_motor.cfg.ctl_mode == Profiled_Velocity)
    {
        return servo_VvelGet();
    }
    else
        return 0;
}

void ServoZapi::servo_doForkLift(int opt)
{
    std_msgs::msg::UInt8 msg;
    switch (opt) {
        case STOP:
            msg.data = 0x0;
            break;
        case PICKUP:
            msg.data = 0x1;
            break;
        case PUTDOWN:
            msg.data = 0x2;
            break;
        default:
            RCLCPP_ERROR(node_->get_logger(), "[ServoZapi] unkown fork lift command [%d]", opt);
            break;
    }

    set_io_output_pub->publish(msg);
}


/*Description: get the motor speed in position control mode*/
uint32_t ServoZapi::servo_PvelGet()
{
    return 0;
}


/*Description: get the motor speed in velocity control mode*/
int ServoZapi::servo_VvelGet()
{
    return pc_.current_vel;
}

bool ServoZapi::Motor_HomeMachine()
{
    return true;
}

bool ServoZapi::Motor_SwitchMode(int8_t mode)
{
    return true;
}

void ServoZapi::Motor_SetAcc(uint32_t acc)
{
    return;
}

void ServoZapi::Motor_SetDec(uint32_t dec)
{
    return;
}

uint16_t ServoZapi::Motor_GetStatusWord()
{
    return 0;
}

double ServoZapi::Motor_GetCurrentPos()
{
    int64_t enc;
    double pos;
    if (!use_extra_encoder) {
        return 0;
    }
    else {
        if (encoder && Motor_GetState() == Ready) {
            double enc_value = encoder->getTurns() * encoder->max_value + encoder->getValue();
            pos = enc_value * pos_scale + pos_offset;
            pc_.timestamp = encoder->timestamp_;
        } else
            pos = 0.0;
        RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
            1000, "[ServoZapi] [%s] get pos from encoder: %.4f", joint_name.c_str(), pos);
    }

    return pos;
}

uint16_t ServoZapi::Motor_GetErrCode()
{
    uint16_t errCode = pc_.error_code;
    
    if(Ready == Motor_GetState())
    {
        if(errCode)
        {
            RCLCPP_WARN_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
                1000, "[ServoZapi] errcode: %d", errCode);
            motorState = Error;
        }
    }
    else {
        if (Error == Motor_GetState() && errCode == 0) {
            motorState = Ready;
            RCLCPP_WARN_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
                1000, "[ServoZapi] errcode clear");   
        }
    }

    return errCode;
}

void ServoZapi::Motor_Brake(bool on)
{
    std_msgs::msg::UInt8 ctlWord_msg;
    if(on)
    {
        ctlWord_msg.data = 0x07;
    }
    else
    {
        ctlWord_msg.data = 0x03;
    }
    set_control_word_pub->publish(ctlWord_msg);
}


void ServoZapi::Motor_Shutdown()
{
    // pCanopen->nmtSend(m_servo_motor.cfg.nodeId, Canopen::State::Stopped);
}

bool ServoZapi::Motor_Halt()
{
    // pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 2, 0x6040, 0x00, 0x0B);   
    return true;
}

bool ServoZapi::Motor_Recover()
{
    return true;
}


std::string ServoZapi::Motor_Err2userErr(const uint16_t& motorErr)
{
    std::string errInfo;
    switch(motorErr)
    {
        case 0x0000: errInfo = "no err"; break;
        case 0x5530: errInfo = "Device hardware data storage EEPROM"; break;
        case 0x4210: errInfo = "Excess temperature device"; break;
        case 0x4310: errInfo = "Motor temperature broked or the motor temperature is too high>=150℃"; break;
        case 0xFF42: errInfo = "Battery is discharged"; break;
        case 0xFF45: errInfo = "Current Gain under acquisition"; break;
        case 0xFF50: errInfo = "Forward and Reverse request direction together"; break;
        case 0x5301: errInfo = "Pal Circuit KO"; break;
        case 0x1000: errInfo = "Generic warning"; break;
        case 0xFFF4: errInfo = "When the input B3 is opened"; break;
        case 0x3223: errInfo = "A11output broken/DC link under-voltage No.3 (manufacturer specific)"; break;
        case 0x4211: errInfo = "Sensor temperatur device broken (manufacturer specific)"; break;
        default:     errInfo = "unknown warning"; break;
    }
    return errInfo;
}

MotorState ServoZapi::Motor_GetState() 
{
    return motorState;
}

double ServoZapi::Motor_GetCurrent()
{
    return 0.0;
}

void ServoZapi::current_vel_sub_callback(const std_msgs::msg::Int16::ConstSharedPtr& msg)
{
    pc_.current_vel = msg->data;
}

void ServoZapi::errcode_sub_callback(const std_msgs::msg::UInt16::ConstSharedPtr& msg)
{
    pc_.error_code = msg->data;
}

void ServoZapi::di_sub_callback(const std_msgs::msg::UInt16::ConstSharedPtr& msg)
{
    pc_.digital_input = msg->data;

    di_interface_pub->publish(*msg);
}

void ServoZapi::custom_operation_sub_callback(const agv_msgs::msg::PeriphCustomOperation::ConstSharedPtr& msg)
{
    servo_doForkLift(msg->operation);
}

}

